package estrategia_OO;

/**
 * LAST UPDATE: 13/10
 */

import lejos.hardware.Sound;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.NXTRegulatedMotor;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

/**
 * Subsistema de navegacao.
 * 
 * @author SEK 2014
 * 
 */
public class Navegacao_OO_D implements Runnable {
	// motores e sensores
	private EV3LargeRegulatedMotor motorEsq;
	private EV3LargeRegulatedMotor motorDir;
	private NXTRegulatedMotor motorPa;
	private EV3TouchSensor sensorEsq;
	private EV3TouchSensor sensorDir;

	// constantes de navegacao
	public final int NORTE = 0;
	public final int SUL = 1;
	public final int LESTE = 2;
	public final int OESTE = 3;

	public final int PARALELO = 4;
	public final int ORTOGONAL = 5;
	public final int CURVA = 6;

	// variaveis
	public boolean fim = false;
	private static Thread ego;
	private int subEstado;
	private int subEstadoAnterior;

	private int listaDeExecucao[] = { NORTE, OESTE, SUL, LESTE };
	private int orientacaoAtual;
	private int tempo;
	private boolean tempoAcabou;
	private int TentativasInseguras;

	// leitura dos sensores de toque
	private boolean toqueEsq;
	private boolean toqueDir;

	private final int velocidadeComum = 350;
	private final int velocidadeCurva = 200;
	private final int velocidadeRe = 350;
	private final int velocidadePa = 80;
	private final int tempoMenor = 5000;
	private final int tempoMaior = 10000;
	private int numeroDeVoltas = 0;

	private void MudarEstado() {
		switch (subEstado) {
		case PARALELO:
			subEstado = ORTOGONAL;
			break;
		case ORTOGONAL:
			subEstado = PARALELO;
			break;
		}
	}

	/**
	 * Metodo get para a variavel orientacao atual do robo
	 * 
	 * @param null
	 * @return orientacao atual do robo
	 */
	public int getOrientacao() {
		return listaDeExecucao[orientacaoAtual];
	}

	public int getProximaOrientacao() {
		if (orientacaoAtual >= 3) {
			return listaDeExecucao[0];
		}

		else
			return listaDeExecucao[orientacaoAtual + 1];
	}

	/**
	 * Metodo get para a variavel toqueEsq
	 * 
	 * @param null
	 * @return variavel toqueEsq
	 */
	public boolean getToqueEsq() {
		return toqueEsq;
	}

	/**
	 * Metodo get para a variavel toqueDir
	 * 
	 * @param null
	 * @return variavel toqueDir
	 */
	public boolean getToqueDir() {
		return toqueDir;
	}

	public int getSubEstado() {
		return subEstado;
	}

	/**
	 * Constroi um subsistema de navegacao do robo
	 * 
	 * @param s1
	 *            Porta de ligacao do sensor de toque esquerdo
	 * @param s2
	 *            Porta de ligacao do sensor de toque direito
	 * @param m1
	 *            Porta de ligacao do motor da roda esquerda
	 * @param m2
	 *            Porta de ligacao do motor da roda direita
	 * @param m3
	 *            Porta de ligacao do motor da pa
	 * @return null
	 */
	public Navegacao_OO_D(Port s1, Port s2, Port m1, Port m2, Port m3) {
		sensorEsq = new EV3TouchSensor(s1);
		sensorDir = new EV3TouchSensor(s2);
		motorEsq = new EV3LargeRegulatedMotor(m1);
		motorDir = new EV3LargeRegulatedMotor(m2);
		motorPa = new NXTRegulatedMotor(m3);

		subEstado = ORTOGONAL;
		motorPa.setSpeed(velocidadePa);
		orientacaoAtual = 0;
		toqueEsq = toqueDir = false;
		tempo = 0;
		tempoAcabou = false;
		TentativasInseguras = 0;
		motorPa.resetTachoCount();

		descePa(false);
		
		Thread teste = new Thread(new Runnable() {
			
			@Override
			public void run() {
				// TODO Auto-generated method stub
				if(motorDir.getSpeed() > velocidadeComum *1.3){
					Sound.beep();
					pararMotor(2);
				} else if(motorEsq.getSpeed() > velocidadeComum * 1.3){
					Sound.beep();
					pararMotor(2);
				}
			}
		});
	}

	/**
	 * Altera a direcao para a proxima da sequencia pre-definida no vetor
	 * listaDeExecucao
	 * 
	 * @param null
	 * @return null
	 */
	private void proximaDirecao() {
		if (orientacaoAtual >= listaDeExecucao.length - 1) {
			orientacaoAtual = 0;
			numeroDeVoltas++;
		} else
			orientacaoAtual++;
		MudarEstado();
		// ego.interrupt();
		if(ego.isInterrupted()){
			iniciaMonitoramento();
		}
	}

	/**
	 * Liga os motores das rodas na direcao para frente
	 * 
	 * @param null
	 * @return null
	 */
	private void moverParaFrente() {
		motorEsq.forward();
		motorDir.forward();
	}

	/**
	 * Liga os motores das rodas na direcao para tras
	 * 
	 * @param null
	 * @return null
	 */
	private void moverParaTras() {
		motorDir.backward();
		motorEsq.backward();
	}

	/**
	 * Faz curva e altera a variavel orientacaoAtual
	 * 
	 * @param null
	 * @return null
	 */
	public void fazerCurva() {
		subEstadoAnterior = subEstado;
		subEstado = CURVA;
		pararMotor(2);
		motorDir.setSpeed(velocidadeCurva);
		motorEsq.setSpeed(velocidadeCurva);
		if (getOrientacao() == OESTE) {
			motorDir.rotate(380, true);
			motorEsq.rotate(-380);
		} else {
			motorDir.rotate(500, false);
		}
		subEstado = subEstadoAnterior;
		proximaDirecao();
	}

	/**
	 * Verifica se houve colisao frontal e atualiza as variaveis
	 * 
	 * @param null
	 * @return null
	 */
	private void amostrarSensorToque() {
		SampleProvider ToqueDir = sensorDir.getTouchMode();
		SampleProvider ToqueEsq = sensorEsq.getTouchMode();
		float amostraDir[] = new float[ToqueDir.sampleSize()];
		float amostraEsq[] = new float[ToqueEsq.sampleSize()];
		ToqueDir.fetchSample(amostraDir, 0);
		ToqueEsq.fetchSample(amostraEsq, 0);

		if (amostraEsq[0] == 1) {
			toqueEsq = true;
		} else {
			toqueEsq = false;
		}
		if (amostraDir[0] == 1) {
			toqueDir = true;
		} else {
			toqueDir = false;
		}
	}

	/**
	 * Implementacao da thread
	 * 
	 * @param null
	 * @return null
	 */
	@Override
	public void run() {
		iniciaMonitoramento();
		while (!fim) {
			if (tempoAcabou) {
//				Sound.beepSequence();
				if (getOrientacao() != OESTE) {
					capturarBolas();
					TentativasInseguras++;
					if (TentativasInseguras >= 3) {
						TentativasInseguras = 0;
						fazerCurva();
					}
				} else {
					fazerCurva();
					TentativasInseguras = 0;
				}

				if(ego.isInterrupted()){
					iniciaMonitoramento();
				}
			}

			amostrarSensorToque();
			if (toqueEsq == false && toqueDir == false) {
				moverParaFrente();
			} else if (toqueEsq == true && toqueDir == true) {
				TentativasInseguras = 0;
				ego.interrupt();
				capturarBolas();
				fazerCurva();
			} else if (toqueEsq == true && toqueDir == false) {
				pararMotor(0);
			} else if (toqueEsq == false && toqueDir == true) {
				pararMotor(1);
			}
			Thread.yield();
			Delay.msDelay(200);
		}
		ego.interrupt();
	}

	/**
	 * Efetua a parada de um motor especifico
	 * 
	 * @param motor
	 *            motor a ser parado (0 = Esq; 1 = Dir; 2 = ambos)
	 * 
	 * @return null
	 */
	private void pararMotor(int motor) {
		if (motor == 0) {
			motorEsq.stop();
		}
		if (motor == 1) {
			motorDir.stop();
		}
		if (motor == 2) {
			motorEsq.stop(true);
			motorDir.stop(true);
		}
	}

	/**
	 * Rotina para captura das bolinhas
	 * 
	 * @param null
	 * 
	 * @return null
	 */
	private void capturarBolas() {
		pararMotor(2);
		Thread movimento = new Thread(new Runnable() {
			@Override
			public void run() {
				// TODO Auto-generated method stub
				motorEsq.setSpeed(170);
				motorDir.setSpeed(170);
				moverParaTras();
				Delay.msDelay(270); // 290
				pararMotor(2);
				moverParaFrente();
				Delay.msDelay(800);
				pararMotor(2);
			}
		});
		movimento.start();
		sobePa(false);

		subEstadoAnterior = subEstado;
		subEstado = CURVA;
		motorEsq.setSpeed(velocidadeRe);
		motorDir.setSpeed(velocidadeRe + 25);
		moverParaTras();
		Delay.msDelay(1000);
		subEstado = subEstadoAnterior;
		pararMotor(2);
		descePa(false);
	}

	/**
	 * Desce a pa do robo para a posicao horizontal
	 * 
	 * @param ret
	 *            retorno imediato
	 * 
	 * @return null
	 */
	private void descePa(boolean ret) {
		motorPa.rotateTo(100, ret);
	}

	/**
	 * Sobe a pa do robo para a posicao vertical
	 * 
	 * @param ret
	 *            retorno imediato
	 * 
	 * @return null
	 */
	private void sobePa(boolean ret) {
		motorPa.rotateTo(-5, ret); // 0
		motorPa.rotateTo(10, ret);
		motorPa.rotateTo(-5, ret);
	}

	private void iniciaMonitoramento() {

		ego = new Thread(new Runnable() {

			@Override
			public void run() {
				// TODO Auto-generated method stub
				switch (getOrientacao()) {
				case NORTE:
					tempo = tempoMenor;
					break;
				case SUL:
					tempo = tempoMenor;
					break;
				case LESTE:
					tempo = tempoMaior / 2;
					break;
				case OESTE:
					if (numeroDeVoltas % 3 == 1) {
						tempo = tempoMaior / 7;
					} else {
						tempo = tempoMaior / 3;
					}

					break;
				}

				if (TentativasInseguras != 0) {
					tempo = tempo / 3;
				}

				try {
					tempoAcabou = false;

					motorDir.setSpeed(velocidadeComum);
					motorEsq.setSpeed(velocidadeComum);

					Thread.sleep(tempo / 4);

					motorEsq.setSpeed((int) ((velocidadeComum) * 1.3));
					motorDir.setSpeed((int) ((velocidadeComum) * 1.3));

					Thread.sleep(tempo / 2);

					motorEsq.setSpeed((int) ((velocidadeComum) * 0.8));
					motorDir.setSpeed((int) ((velocidadeComum) * 0.8));

					Thread.sleep(tempo / 4);

					tempoAcabou = true;

				} catch (InterruptedException ie) {
					pararMotor(2);
					motorDir.setSpeed(0);
					motorEsq.setSpeed(0);
					Thread.currentThread().interrupt();
					return;
				}
			}
		});
		ego.start();
	}
}